#include "mavlink_interface.hpp"
#include <bot_core/bot_core.h>
#include <bot_param/param_client.h>
#include <sys/ioctl.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string>
#include <limits>

#include <lcm/lcm-cpp.hpp>
#include <lcmtypes/asctec_sdk_driver.hpp>

#include <ConciseArgs>

#define READ_BUFFER_SIZE 128

namespace asctec_sdk_driver {

class App {
 public:
  App(int argc, char ** argv);
  ~App();

  void control_command_handler(const lcm::ReceiveBuffer* rbuf,
                               const std::string& chan,
                               const quad_control_msg_t* msg);

  void motor_command_handler(const lcm::ReceiveBuffer* rbuf,
                             const std::string& chan,
                             const motor_commands_t* msg);

  void read_serial_data();
  void send_mode_command();

 private:
  lcm::LCM * lcm;

  std::string serial_device;
  int baud;
  mavlink_message_t msg;
  mavlink_status_t status;
  int packet_drops;
  int fd;
  bot_timestamp_sync_state_t * timesync;
  uint8_t * read_buffer;

  BotParam * param;

  int control_mode;

  int imu_pub_period;
  int motor_rpm_pub_period;
  int rc_data_pub_period;
  int gps_data_pub_period;

};

static gboolean serialReadHandler(GIOChannel *source, GIOCondition condition,
                                  gpointer user_data) {
  App * self = (App *) user_data;
  self->read_serial_data();
  return TRUE;
}

static gboolean mode_timer_handler(gpointer user) {
  App * self = (App *) user;
  self->send_mode_command();
  return TRUE;
}

void App::send_mode_command() {
  mavlink_asctec_api_command_t api_command;
  api_command.control_mode = control_mode;
  api_command.publishing_mode = ASCTEC_PUB_ON;
  api_command.imu_pub_period = imu_pub_period;
  api_command.motor_rpm_pub_period = motor_rpm_pub_period;
  api_command.rc_data_pub_period = rc_data_pub_period;
  api_command.gps_data_pub_period = gps_data_pub_period;
  send_api_command(fd, &api_command);
}

void App::read_serial_data() {
  while (1) {
    int bytes_available = bot_serial_bytes_available(fd);
    if (bytes_available <= 0)
      break;

    int num_to_read = bot_min(bytes_available,READ_BUFFER_SIZE);
    int num_read = read(fd, read_buffer, num_to_read);
    if (num_read != num_to_read) {
      fprintf(stderr, "warning, read %d of %d available bytes\n", num_read,
              num_to_read);
    } else if (num_read <= 0) {
      fprintf(stderr,
              "Error: number of bytes read != number of bytes to read\n");
      continue;
    }

    for (int i = 0; i < num_read; i++) {

      int64_t recv_time = bot_timestamp_now();
      int64_t synced_time = recv_time;

      if (mavlink_parse_char(MAVLINK_COMM_0, read_buffer[i], &msg, &status)) {
        switch (msg.msgid) {
          case MAVLINK_MSG_ID_ASCTEC_IMU: {
            mavlink_asctec_imu_t imu;
            mavlink_msg_asctec_imu_decode(&msg, &imu);
            synced_time = bot_timestamp_sync(timesync, imu.msec, recv_time);
            publish_imu_message(lcm, &imu, synced_time);
          }
            break;

          case MAVLINK_MSG_ID_ASCTEC_MOTOR_RPM: {
            mavlink_asctec_motor_rpm_t rpm;
            mavlink_msg_asctec_motor_rpm_decode(&msg, &rpm);
            synced_time = bot_timestamp_sync(timesync, rpm.msec, recv_time);
            publish_motor_rpm_message(lcm, &rpm, synced_time);
          }
            break;

          case MAVLINK_MSG_ID_ASCTEC_GPS: {
            mavlink_asctec_gps_t gps;
            mavlink_msg_asctec_gps_decode(&msg, &gps);
            synced_time = bot_timestamp_sync(timesync, gps.msec, recv_time);
            publish_gps_message(lcm, &gps, synced_time);
          }
            break;

          case MAVLINK_MSG_ID_ASCTEC_RC_DATA: {  //  char * dev = "/dev/ttyUSB0";
            mavlink_asctec_rc_data_t rc_channels;
            mavlink_msg_asctec_rc_data_decode(&msg, &rc_channels);
            synced_time = bot_timestamp_sync(timesync, rc_channels.msec,
                                             recv_time);
            publish_rc_channels_message(lcm, &rc_channels, synced_time);
          }
            break;

          case MAVLINK_MSG_ID_ASCTEC_HEARTBEAT: {
            mavlink_asctec_heartbeat_t heartbeat;
            mavlink_msg_asctec_heartbeat_decode(&msg, &heartbeat);
            synced_time = bot_timestamp_sync(timesync, heartbeat.msec,
                                             recv_time);
            publish_heartbeat_message(lcm, &heartbeat, synced_time, recv_time);
            fprintf(stderr, ".");
          }
            break;
        }
      }
    }
  }
//  packet_drops += status.packet_rx_drop_count;
  return;
}

void App::control_command_handler(const lcm::ReceiveBuffer* rbuf,
                                  const std::string& chan,
                                  const quad_control_msg_t* msg) {
  send_stick_commands(fd, msg);
}

void App::motor_command_handler(const lcm::ReceiveBuffer *rbuf,
                                const std::string& chan,
                                const motor_commands_t * msg) {
  send_motor_commands(fd, msg);
}

App::App(int argc, char ** argv) {
  serial_device = "/dev/ttyUSB0";
  baud = 230400;
  imu_pub_period = 10;
  motor_rpm_pub_period = 10;
  rc_data_pub_period = 0;
  gps_data_pub_period = 250;

  control_mode = ASCTEC_COMMAND_MODE_RC;
//  control_mode = ASCTEC_COMMAND_MODE_ATTITUDE;
  ConciseArgs args(argc, argv);
  args.add(serial_device, "d", "device", "Serial device to open");
  args.add(baud, "b", "baud", "Baud rate for serial device");
  args.add(imu_pub_period, "i", "imu_pub_period",
           "Publishing period for IMU data (in ms)");
  args.add(motor_rpm_pub_period, "m", "motor_rpm_pub_period",
           "Publishing period for motor rpm data (in ms)");
  args.add(rc_data_pub_period, "r", "rc_data_pub_period",
           "Publishing period for rc data (in ms), <=0 to disable");
  args.add(gps_data_pub_period, "g", "gps_data_pub_period",
           "Publishing period for gps data (in ms)");
  args.parse();

  fd = bot_serial_open(serial_device.c_str(), baud, 1);

  lcm = new lcm::LCM();

  read_buffer = new uint8_t[READ_BUFFER_SIZE];

  // Send an API_COMMAND message periodically
  g_timeout_add(1000, mode_timer_handler, this);

  bot_glib_mainloop_attach_lcm(lcm->getUnderlyingLCM());
  timesync = bot_timestamp_sync_init(1000, std::numeric_limits<uint32_t>::max(),
                                     1.01);

  lcm->subscribe("CONTROL_COMMAND", &App::control_command_handler, this);
//  lcm->subscribe("MOTOR_COMMAND", &App::motor_command_handler, this);

  GIOChannel * ioc = g_io_channel_unix_new(fd);

  g_io_add_watch_full(ioc, G_PRIORITY_HIGH, G_IO_IN,
                      (GIOFunc) serialReadHandler, this, NULL);

}
App::~App() {
  delete lcm;
  delete[] read_buffer;
}

}  // namespace asctec_sdk_driver

int main(int argc, char ** argv) {

  GMainLoop * mainloop = g_main_loop_new(NULL, FALSE);
  bot_signal_pipe_glib_quit_on_kill(mainloop);

  asctec_sdk_driver::App self(argc, argv);

  g_main_loop_run(mainloop);

  return 0;
}
